// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

// 不要升级WPILib到新版本！不要升级WPILib到新版本！不要升级WPILib到新版本！

// 新设备如果遇到头文件错误请右键左侧build.gradle文件-管理外部供应库-在线安装新的库
// 然后把下面两个网址贴进去，回车
// 请不要把开头的名字贴进去
// REV Robotics REVLib https://software-metadata.revrobotics.com/REVLib.json
// CTRE Phoenix Framework https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix-frc2022-latest.json

// 如果你从未使用过此设备部署过代码，或者此设备已经很久没有部署过代码了，请在联网环境下拉取并编译一次代码

#include "Robot.h"

#include <fmt/core.h>

#include <frc/smartdashboard/SmartDashboard.h>

#include <math.h> //数学类头文件

#include <frc/controller/PIDController.h> // PID控制 https://docs.wpilib.org/en/stable/docs/software/advanced-controls/controllers/pidcontroller.html
#include <frc/Joystick.h>				  //手柄操纵的库
#include <frc/TimedRobot.h>				  //用于TimedRobot的库
#include <frc/Timer.h>					  //计时器库
#include <frc/drive/DifferentialDrive.h>  //一些驱动
#include <frc/motorcontrol/PWMSparkMax.h> //PWMSparkMax的控制库
#include <frc/Timer.h>					  // 计时器

#include <cameraserver/CameraServer.h> //摄像头

#include <frc/Solenoid.h> //电磁阀

// Phoenix的电机库文件
#include <ctre/phoenix/motorcontrol/can/TalonFX.h>

#include <stdio.h> //标准输入输出头文件

#include <frc/ADXRS450_Gyro.h> //陀螺仪的库

#include <frc/AddressableLED.h> //LED的库

#include "MiniPID.h" //PID 库

#include <unistd.h>
#include <iostream>

// 定义左右电机buffer数组
int buffer_left[50] = {0};
int buffer_right[50] = {0};
// 记录左右buffer的读入位置
int buffer_flag = 0;
const int max_length = 5; // 限制数组的长度为5

void Robot::RobotInit()
{ // 初始化程序，在每一次启动的时候执行一遍
	m_chooser.SetDefaultOption(kAutoNameDefault, kAutoNameDefault);
	m_chooser.AddOption(kAutoNameCustom, kAutoNameCustom);
	frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
	// Robot() : frc::TimedRobot(20_ms) {}//设置扫描周期，默认为20ms一次

	// 这里的反向其实不推荐打开
	// 因为后面还需要对速度数值进行一些计算
	// 负数计算会比较麻烦而且难以控制
	// 在控制电机的时候统一反向就好了
	//  motor_0.SetInverted(true);//电机反过来，不然会转圈
	//  motor_2.SetInverted(true);//电机反过来，不然会转圈

	// 陀螺仪校准
	// 请注意校准时不能移动（开机时校准）
	m_gyro.Calibrate();

	frc::CameraServer::StartAutomaticCapture(); // 摄像头

	// 初始化LED
	m_led.SetLength(kLength);
	m_led.SetData(m_ledBuffer);
	m_led.Start();
	// 将LED全部设置为绿色
	for (int i = 0; i < kLength; i++)
	{
		m_ledBuffer[i].SetRGB(0, 255, 0);
	}
	m_led.SetData(m_ledBuffer);
}

/**
 * This function is called every robot packet, no matter the mode. Use
 * this for items like diagnostics that you want ran during disabled,
 * autonomous, teleoperated and test.
 *
 * <p> This runs after the mode specific periodic functions, but before
 * LiveWindow and SmartDashboard integrated updating.
 */
void Robot::RobotPeriodic()
{ // 机器人的全局定时扫描
}

/**
 * This autonomous (along with the chooser code above) shows how to select
 * between different autonomous modes using the dashboard. The sendable chooser
 * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
 * remove all of the chooser code and uncomment the GetString line to get the
 * auto name from the text box below the Gyro.
 *
 * You can add additional auto modes by adding additional comparisons to the
 * if-else structure below with additional strings. If using the SendableChooser
 * make sure to add them to the chooser code above as well.
 */
void Robot::AutonomousInit()
{ // 自动化控制，初始化设置
	// m_autoSelected = m_chooser.GetSelected();
	// m_autoSelected = SmartDashboard::GetString("Auto Selector",
	//                                            kAutoNameDefault);
	// fmt::print("Auto selected: {}\n", m_autoSelected);

	// if (m_autoSelected == kAutoNameCustom)
	// {
	// 	// Custom Auto goes here
	// }
	// else
	// {
	// 	// Default Auto goes here
	// }

	// 陀螺仪校准
	// 请注意校准时不能移动（自动化开始时校准）
	// 校准占用自动化阶段的时间
	// m_gyro.Calibrate();

	// 初始化计时器
	m_timer.Reset();
	m_timer.Start();
}

/**
 * 不需要自己开循环
 * 程序会自动循环调用
 */
void Robot::AutonomousPeriodic()
{ // 自动化控制，定时扫描

	// Custom Auto goes here
	// 下方数字以毫秒计算
	// 设置初始速度
	double autoSpeed = -0.5;

	if (m_timer.Get() < 5_s)
	{
		SetGyro = 0.0; // 首先直线走
		// 用PID控制车辆以直线运动
		rightSpeed = autoSpeed + m_pid.Calculate(m_gyro.GetAngle(), SetGyro); // 右侧电机方向反转
		// std::cout << "右侧电机速度：" << rightSpeed << std::endl;
		leftSpeed = autoSpeed + m_pid.Calculate(m_gyro.GetAngle(), SetGyro);
		// std::cout << "左侧电机速度：" << leftSpeed << std::endl;
		motor_0.Set(-rightSpeed);
		motor_2.Set(-rightSpeed);
		motor_1.Set(leftSpeed);
		motor_3.Set(leftSpeed);
		// sleep(20); // 暂停一下防止卡死
	}
	// 停车
	else
	{
		motor_0.Set(0);
		motor_2.Set(0);
		motor_1.Set(0);
		motor_3.Set(0);
	}
}

void Robot::TeleopInit()
{ // 远程操作的操纵杆控制，初始化设置
  //  //保障在遥控模式下自动化程序关闭
  //  if (m_autoSelected != nullptr)
  //  {
  //    m_autoSelected->Cancel();
  //    m_autoSelected = nullptr;
  //  }
}

void Robot::TeleopPeriodic()
{ // 远程操作的操作杆控制，定期扫描
	// 这边每次都会初始化一次归零
	// 目的是因为手柄回正时会有一点飘移
	// 这样会导致后面LTRT加速判断方向的时候出现错误
	// 所以在每一次循环的时候都需要重新初始化一下

	// 请注意每一次循环都初始化是有必要的
	// 不能在Robot.h里定义的时候初始化
	// 因为这样会导致速度值不断累加无法控制
	leftSpeed = 0.0;
	rightSpeed = 0.0;

	// PS4 手柄
	// 获取手柄左手摇杆的XY轴
	PS4_L_Joy_x = PS4.GetLeftX();
	PS4_L_Joy_y = -PS4.GetLeftY();
	// 获取手柄右手摇杆的XY轴
	PS4_R_Joy_x = PS4.GetRightX();
	PS4_R_Joy_y = -PS4.GetRightY();
	// 获取手柄L2和R2肩键
	//  PS4_L2 = PS4.GetL2Axis();
	//  PS4_R2 = PS4.GetR2Axis();
	// 获取手柄L1和R1键
	PS4_L1 = PS4.GetL1Button();
	PS4_R1 = PS4.GetR1Button();
	// 获取手柄左右摇杆按下状态L3和R3
	PS4_L3 = PS4.GetL3Button();
	PS4_R3 = PS4.GetR3Button();
	// 获取手柄ABXY键，为啥PS布局却使用了Xbox的键位请看定义处的注释
	PS4_A = PS4.GetCrossButton();
	PS4_B = PS4.GetCircleButton();
	PS4_X = PS4.GetSquareButton();
	PS4_Y = PS4.GetTriangleButton();
	// 获取手柄功能键Share和Options
	PS4_Share = PS4.GetShareButton();
	PS4_Options = PS4.GetOptionsButton();

	// Xbox 手柄
	// 获取手柄左手摇杆的XY轴
	// PS4_L_Joy_x = Xbox.GetLeftX();
	// PS4_L_Joy_y = -Xbox.GetLeftY();
	// //获取手柄右手摇杆的XY轴
	// PS4_R_Joy_x = Xbox.GetRightX();
	// PS4_R_Joy_y = -Xbox.GetRightY();
	// //获取手柄LT和RT肩键
	// PS4_L2 = Xbox.GetLeftTriggerAxis();
	// PS4_R2 = Xbox.GetRightTriggerAxis();
	// //获取手柄左右摇杆按下状态LSB和RSB
	// PS4_L3 = Xbox.GetLeftStickButton();
	// PS4_R3 = Xbox.GetRightStickButton();
	// //获取手柄ABXY键
	// PS4_A = Xbox.GetAButton();
	// PS4_B = Xbox.GetBButton();
	// PS4_X = Xbox.GetXButton();
	// PS4_Y = Xbox.GetYButton();
	// //获取手柄LB和RB键
	// PS4_L1 = Xbox.GetLeftBumper();
	// PS4_R1 = Xbox.GetRightBumper();

	// LTRT一键加速
	// 怠速情况下默认是LTRT控制向前加速
	// 请注意操作时要使用+=，直接使用=会导致前面的处理被抹掉
	// 虽然说这边用==应该问题也不大
	leftSpeed += leftSpeed >= 0 ? PS4_L2 : -PS4_L2; // 如果大于零就加上LT的数值，小于零就减去
	rightSpeed += rightSpeed >= 0 ? PS4_R2 : -PS4_R2;

	// 摇杆控制速度
	// 这边用了一些稍微复杂一点的函数控制
	// 感觉可以优化一些，有点太复杂了
	// 这边的意思是在油门空速的情况下，转向速度为0.3
	// 油门满速的情况下，转向速度为1.2
	// 也就是说可以线性调整转向速度
	// 在油门一直推满的情况下依旧可以灵活转向
	// 即使油门拉满依旧能让两个轮子相反运动
	// 解释一下这段调制在干啥：
	// 相当于y = 0.9|x| + 0.3
	// 想要调整两种转速请自行把所有的1.2和0.3进行替换
	//  1.0为基数不要动，表示油门在-1.0到1.0之间运动
	leftSpeed += PS4_L_Joy_y + PS4_R_Joy_x * ((PS4_L_Joy_y) * ((1.2 - 0.3) / 1.0) + 0.3); // 加减控制左右转
	rightSpeed += PS4_L_Joy_y - PS4_R_Joy_x * ((PS4_L_Joy_y) * ((1.2 - 0.3) / 1.0) + 0.3);

	// // LBRB一键刹车
	// if (PS4_L1 == true)
	// {
	//   leftSpeed = 0;
	// }
	// if (PS4_R1 == true)
	// {
	//   rightSpeed = 0;
	// }

	// 限制最大值
	//  leftSpeed = leftSpeed > 1.0 ? 1.0 : leftSpeed;
	//  leftSpeed = leftSpeed < -1.0 ? -1.0 : leftSpeed;
	//  rightSpeed = rightSpeed > 1.0 ? 1.0 : rightSpeed;
	//  rightSpeed = rightSpeed < -1.0 ? -1.0 : rightSpeed;

	// 限速
	leftSpeed = leftSpeed * 0.6;
	rightSpeed = rightSpeed * 0.6;

	// 对操作进行滤波
	// 因为不可能超过+-1.0所以max和min开的不大
	// 注意比较的时候是用的绝对值比较的
	//  double max_value_left = 0.0, min_value_left = 20.0; //定义最大值最小值，并且初始化
	//  double max_value_right = 0.0, min_value_right = 20.0;
	//  //控制flag在0-max_length-1之间循环
	//  buffer_flag %= max_length;
	//  //控制flag+1，使得数组在1-max_length之间循环
	//  buffer_left[buffer_flag + 1] = leftSpeed;
	//  buffer_right[buffer_flag + 1] = rightSpeed;
	//  for (int i = 1; i <= max_length; i++)
	//  { //遍历一遍扫描最大最小值
	//    max_value_left = max_value_left > buffer_left[i] ? max_value_left : buffer_left[i];
	//    min_value_left = min_value_left < buffer_left[i] ? min_value_left : buffer_left[i];
	//    //累加数组中所有的数字
	//    return_left += buffer_left[i];
	//  }
	//  for (int i = 1; i <= max_length; i++)
	//  {
	//    max_value_right = max_value_right > buffer_right[i] ? max_value_right : buffer_right[i];
	//    min_value_right = min_value_right < buffer_right[i] ? min_value_right : buffer_right[i];
	//    return_right += buffer_right[i];
	//  }
	//  //返回值等于去除最大最小的数组中的数字的平均值
	//  return_left = (return_left - max_value_left - min_value_left) / (max_length - 2);
	//  return_right = (return_right - max_value_right - min_value_right) / (max_length - 2);
	//  //把滤过的波的输入压入buffer中
	//  // buffer_left[buffer_flag] = return_left;
	//  // buffer_right[buffer_flag] = return_right;
	//  buffer_flag++;
	//  // printf("%lf\n", return_left);

	// rightSpeed = return_right;
	// leftSpeed = return_left;

	// 操纵马达驱动，注意这边速度设置不能搞错了，不然会对转打齿
	// motor_0.Set(-rightSpeed);
	// motor_2.Set(-rightSpeed);
	// motor_1.Set(leftSpeed);
	// motor_3.Set(leftSpeed);

	// 操纵电磁阀放气
	Solenoid_1.Set(PS4_Y); // 当Y按下时电磁阀1放气
	Solenoid_2.Set(PS4_X); // 当X按下时电磁阀2放气

	motor_launchBall.Set(ControlMode::PercentOutput, -PS4_A * 0.75); // 发球，用A控制

	send_ball_0.Set(-PS4_B * 1.0);	// 传球皮带电机，用B控制
	send_ball_1.Set(PS4_L1 * 1.0); // 传球独立轮组电机，用L1控制

	collect_holder.Set(PS4_Share * 1.0); // 收球支架，用左上小键Back控制

	collect_gear.Set(-(PS4_L_Joy_y)); // 收球轮，用左摇杆控制

	// send_ball_0.Set(ControlMode::PercentOutput, PS4_R_Joy_y); // 传球独立轮组电机，用右摇杆控制
}

void Robot::DisabledInit()
{
}

void Robot::DisabledPeriodic()
{
}

void Robot::TestInit()
{ // 测试模式，初始化设置
}

void Robot::TestPeriodic()
{
	// 定速50%测试
	//  motor_0.Set(0.5);
	//  motor_2.Set(0.5);
	//  motor_1.Set(0.5);
	//  motor_3.Set(0.5);
}

#ifndef RUNNING_FRC_TESTS
int main()
{
	return frc::StartRobot<Robot>();
}
#endif